import cv2 as cv
import numpy as np
img1 = cv.imread('D:\photo\I7 PLUS\est\IMG_4328.JPG')
img2 = cv.imread('D:\photo\I7 PLUS\est\IMG_4335.JPG')
img1 = cv.resize(img1,None,fx=0.2,fy=0.2)
img2 = cv.resize(img2,None,fx=0.2,fy=0.2)
gray_1 = cv.cvtColor(img1,cv.COLOR_BGR2GRAY)
gray_2 = cv.cvtColor(img2,cv.COLOR_BGR2GRAY)
corners_1 = cv.goodFeaturesToTrack(gray_1,8000,0.01,2)
corners_2 = cv.goodFeaturesToTrack(gray_2,8000,0.01,2)
pts_1 = []
pts_2 = []

for i in corners_1:
    x_1,y_1 = i.ravel()
    cv.circle(img1,(x_1,y_1),2,(255,0,255),1)
    pts_1.append((x_1,y_1))
#print(pts_1)

for m in corners_2:
    x_2,y_2 = m.ravel()
    cv.circle(img2,(x_2,y_2),2,(0,255,255),1)
    pts_2.append((x_2,y_2))
'''cv.imshow('img1',img1)
cv.imshow('img2',img2)
cv.waitKey()'''
pts_1 = np.array(pts_1,np.float32)
pts_2 = np.array(pts_2,np.float32)
print(pts_2)

pts_1 = pts_1[0:800,:]
pts_2 = pts_2[0:800,:]
cameraMatrix = np.array([[1,0,0],[0,1,0],[0,0,1]])
E,mask = cv.findEssentialMat(pts_1,pts_2,cameraMatrix,cv.FM_RANSAC)
print(E)

U,M,V = np.linalg.svd(E)
R_positive = np.array([[0,-1,0],[1,0,0],[0,0,1]])
t_x = np.dot(U,R_positive)
t_x = np.dot(t_x,M)
t = np.dot(t_x,U.T)

R = np.dot(U,R_positive.T)
R = np.dot(R,M)
R = np.dot(R,V.T)
print('T\n',t,'\n','R\n',R)
